/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package flightlifter;

import java.util.Vector;

/**
 *
 * @author Matt
 */
public class OrthoNormalBasis {

    Point front, middle, up;
    PVector upV, rightV, forwardV;

    public OrthoNormalBasis(Point front, Point middle, Point up) {
        this.front = front;
        this.middle = middle;
        this.up = up;
    }

    public void update() {
        forwardV = getForward();
        rightV = getRight();
        upV = getUp();
    }

    public String pickle(Vector<Node> nodes) {
        return nodes.indexOf(front) + ","
                + nodes.indexOf(middle) + ","
                + nodes.indexOf(up) + ",";
    }

    public PVector getForward() {
        return PVector.sub(front.pos, middle.pos);
    }

    public PVector getUp() {
        return PVector.sub(up.pos, middle.pos);
    }

    public PVector getRight() {
        PVector ret = new PVector();

        PVector.cross(getUp(), getForward(), ret);
        return ret;
    }

    public PVector getForwardCentroid() {
        return new PVector((front.pos.x + middle.pos.x) / 2,
                (front.pos.y + middle.pos.y) / 2,
                (front.pos.z + middle.pos.z) / 2);
    }

    public PVector getCentroid() {


        return new PVector((front.pos.x + middle.pos.x + up.pos.x) / 3,
                (front.pos.y + middle.pos.y + up.pos.y) / 3,
                (front.pos.z + middle.pos.z + up.pos.z) / 3);
    }

    public float getRotX() {
        //angle between line pointing down Z axis and our forward vector
        PVector forward = getForward();
        forward.normalize();
        return (float) (Math.atan2(0, 1) - Math.atan2(forward.y, forward.z)) * 57.295f;

    }

    public float getRotY() {
        //angle between line pointing down Z axis and our forward vector
        PVector forward = getForward();
        forward.normalize();
        return (float) (Math.atan2(1,0) - Math.atan2(forward.z, forward.x)) * 57.295f;

    }
    public float getRotZ() {
        //angle between line pointing down Z axis and our forward vector
        PVector forward = getUp();
        forward.normalize();
        return (float) (Math.atan2(0, 1) - Math.atan2(forward.y, forward.x)) * 57.295f;

    }
}
